#ifndef OBJECT_3D_UTILS
#define OBJECT_3D_UTILS

#include "../D3DUtil.h"
#include "Object3D.h"
#include "Object3DCollider.h"
#include <iostream>

namespace dkm
{
	struct AABB
	{
		D3DXVECTOR3 min;
		D3DXVECTOR3 max;
	};

	//Returns closest point on an aligned axis bounding box.
	static D3DXVECTOR3 ClosestPointOnAABB(const D3DXVECTOR3 &point, const AABB &aabb)
	{
		D3DXVECTOR3 closestPoint;
		closestPoint.x = (point.x < aabb.min.x)? aabb.min.x : (point.x > aabb.max.x)? aabb.max.x : point.x;
		closestPoint.y = (point.y < aabb.min.y)? aabb.min.y : (point.y > aabb.max.y)? aabb.max.y : point.y;
		closestPoint.z = (point.z < aabb.min.z)? aabb.min.z : (point.z > aabb.max.z)? aabb.max.z : point.z;

		return closestPoint;
	}

	//Returns true if the two aligned axis bounding boxes collide.
	static bool TestBoxBox(const AABB &aabb1, const AABB &aabb2)
	{
		return	(aabb1.max.x >= aabb2.min.x && aabb1.min.x <= aabb2.max.x)
			 && (aabb1.max.y >= aabb2.min.y && aabb1.min.y <= aabb2.max.y)
			 && (aabb1.max.z >= aabb2.min.z && aabb1.min.z <= aabb2.max.z);
	}

	//Returns true if the two spheres collide.
	static bool TestSphereSphere(const D3DXVECTOR3 &center1, const float radius1, const D3DXVECTOR3 &center2, const float radius2)
	{
		return (D3DXVec3LengthSq(&(center1-center2)) <= (radius1+radius2)*(radius1+radius2));
	}

	//Returns true if the sphere and the aligned axis bounding box collide.
	static bool TestSphereBox(const D3DXVECTOR3 &sphereCenter, const float sphereRadius, AABB &aabb)
	{
		D3DXVECTOR3 closestPoint = ClosestPointOnAABB(sphereCenter, aabb);
		if (D3DXVec3LengthSq(&(sphereCenter-closestPoint)) <= sphereRadius*sphereRadius)
		{
			return true;
		}
		return false;
	}

	//Returns true if the two objects collide.
	//Returns false if an object does not have a collider.
	static bool CheckCollision3D(Object3D* object1, Object3D* object2)
	{
		if (!object1->HasCollider() || !object2->HasCollider()) return false;
		if (object1->Collider()->GetShape() == Object3DCollider::SPHERE)
		{
			if (object2->Collider()->GetShape() == Object3DCollider::SPHERE)
			{
				return TestSphereSphere(*(object1->Position())+*(object1->Collider()->Center()), object1->Collider()->Radius(), *(object2->Position())+*(object2->Collider()->Center()),object2->Collider()->Radius());				
			}
			else
			{
				AABB aabb;
				aabb.min = (*(object2->Position())+*(object2->Collider()->Center())) - D3DXVECTOR3(object2->Collider()->Length()*0.5f,object2->Collider()->Height()*0.5f,object2->Collider()->Width()*0.5f);
				aabb.max = aabb.min + D3DXVECTOR3(object2->Collider()->Length(),object2->Collider()->Height(),object2->Collider()->Width());
				return TestSphereBox(*(object1->Position())+*(object1->Collider()->Center()), object1->Collider()->Radius(), aabb);
			}
		}
		else
		{
			if (object2->Collider()->GetShape() == Object3DCollider::SPHERE)
			{
				AABB aabb;
				aabb.min = (*(object1->Position())+*(object1->Collider()->Center())) - D3DXVECTOR3(object1->Collider()->Length()*0.5f,object1->Collider()->Height()*0.5f,object1->Collider()->Width()*0.5f);
				aabb.max = aabb.min + D3DXVECTOR3(object1->Collider()->Length(),object1->Collider()->Height(),object1->Collider()->Width());
				return TestSphereBox(*(object2->Position())+*(object2->Collider()->Center()), object2->Collider()->Radius(), aabb);
			}
			else
			{
				AABB aabb1;
				aabb1.min = (*(object1->Position())+*(object1->Collider()->Center())) - D3DXVECTOR3(object1->Collider()->Length()*0.5f,object1->Collider()->Height()*0.5f,object1->Collider()->Width()*0.5f);
				aabb1.max = aabb1.min + D3DXVECTOR3(object1->Collider()->Length(),object1->Collider()->Height(),object1->Collider()->Width());
				AABB aabb2;
				aabb2.min = (*(object2->Position())+*(object2->Collider()->Center())) - D3DXVECTOR3(object2->Collider()->Length()*0.5f,object2->Collider()->Height()*0.5f,object2->Collider()->Width()*0.5f);
				aabb2.max = aabb2.min + D3DXVECTOR3(object2->Collider()->Length(),object2->Collider()->Height(),object2->Collider()->Width());
				return TestBoxBox(aabb1,aabb2);
			}
		}
	}	
}

#endif